#! /usr/bin/python3
import threading
import time
import cv2
from flask import Flask,render_template,Response,request
import numpy as np
import matplotlib.pyplot as plt
plt.switch_backend('agg') #不加这句可能出现mainloop not in mainthread
from matplotlib.patches import Ellipse
import base64
from io import BytesIO
import signal, sys
import rospy 
from dip_msg.msg import visualize_msg,web_command_msg
from cv_bridge import CvBridge,CvBridgeError
from sensor_msgs.msg import Image as ROSImage



app = Flask(__name__)

class RangeParam(object):
    def __init__(self,min_val:float,max_val:float,step_val:float,current_val=0):
        self.min_value = min_val
        self.max_value = max_val
        self.step_value = step_val
        self.current_value = current_val

#---------------index page param-------------------
#下位机属性
Kp = RangeParam(0,10,0.1,1)
Ki = RangeParam(0,10,0.1,0)
Kd = RangeParam(0,10,0.1,0)

plc_param_dict = {
    "Kp":Kp,"Ki":Ki,"Kd":Kd
}
plc_range_params_info = plc_param_dict.items()
plc_range_params_name = list(plc_param_dict.keys())
#上位机属性
pc_parameter_1 = RangeParam(-1,1,0.1,0)
pc_parameter_2 = RangeParam(0,1,0.01,0.5)

pc_param_dict = {
    "pc_parameter_1":pc_parameter_1,
    "pc_parameter_2":pc_parameter_2
}
pc_range_params_info = pc_param_dict.items()
pc_range_params_name = list(pc_param_dict.keys())

#TODO 这里应该有更好的实现方式，为了简单我直接用了exec
index_range_param_dict = {
    "plc_param_dict":plc_range_params_name,
    "pc_param_dict":pc_range_params_name}

#--------------video page param--------------------------
left_h_min = RangeParam(0,255,1,128)
left_h_max = RangeParam(0,255,1,128)
right_h_min = RangeParam(0,255,1,128)
right_h_max = RangeParam(0,255,1,128)
s_min = RangeParam(0,255,1,128)
s_max = RangeParam(0,255,1,128)
v_min = RangeParam(0,255,1,128)
v_max = RangeParam(0,255,1,128)

img_param_dict = {
    "left_h_min":left_h_min,
    "left_h_max":left_h_max,
    "right_h_min":right_h_min,
    "right_h_max":right_h_max,
    "s_min":s_min,
    "s_max":s_max,
    "v_min":v_min,
    "v_max":v_max
}
img_range_params_info = img_param_dict.items()
img_range_params_name = list(img_param_dict.keys())
UPDATE_PARAMS = False #是否更新参数
VISUALIZE_DATA = visualize_msg() #接受信息
WEB_COMMAND = web_command_msg() #发送指令
visualize_img = None #可视化图像
visualize_img = cv2.resize(cv2.imread("/home/lin/hitsz-dip/test.png"),(320,240))
localization_buffer = BytesIO() #定位可视化
fig,ax = plt.subplots(figsize=(6,4))

# def ImageCallback(data,enable_debug = False):
#     """
#     Brief:
#       图像接收回调
#     """
#     global visualize_img
#     brigde = CvBridge()
#     visualize_img = brigde.imgmsg_to_cv2(data,"bgr8")
#     if (enable_debug):
#         cv2.imshow("debug",visualize_img)
#         cv2.waitKey(0)

def VisualizeMsgCallback(data):
    """
    Brief:
      可视化消息回调,生成matplotlib图像
    """
    global localization_buffer,VISUALIZE_DATA
    global fig,ax
    localization_buffer = BytesIO()
    plt.cla()
    VISUALIZE_DATA = data
    x = data.camera_pose_x
    y = data.camera_pose_y
    yaw = data.camera_pose_theta
    plt.xlim((-3,3))
    plt.ylim((-2,2))
    ax.scatter([0,0.8],[0.0,0],c="r",label = "column")
    ax.quiver(0,0,0.5,0,angles="xy",color = "y",scale_units="xy",scale=1)
    ax.quiver(0,0,0,0.5,angles="xy",color = "r",scale_units="xy",scale=1)
    ax.scatter(x,y,c = "c",label="observe  coords")
    ax.scatter(data.predict_x,data.predict_y,c = "m",label="predict  coords")
    ax.quiver(data.predict_x,data.predict_y,0.5*np.cos(data.predict_yaw),0.5*np.sin(data.predict_yaw),color="y",angles="xy",scale_units="xy",scale=1)
    ax.quiver(data.predict_x,data.predict_y,-0.5*np.sin(data.predict_yaw),0.5*np.cos(data.predict_yaw),color="r",angles="xy",scale_units="xy",scale=1)
    #x = cosx -siny,
    ax.quiver(x,y,0.5*np.cos(yaw),0.5*np.sin(yaw),color="m",angles="xy",scale_units="xy",scale=1)
    ax.quiver(x,y,-0.5*np.sin(yaw),0.5*np.cos(yaw),color="b",angles="xy",scale_units="xy",scale=1)
    ax.scatter(data.filter_x,data.filter_y,c="g",label="filter coords")

    cov_ellipse = Ellipse([x,y],data.cov_x,data.cov_y,angle=yaw)
    cov_ellipse.set_alpha(0.5)
    cov_ellipse.set_color("c")
    ax.add_patch(cov_ellipse)
    # plt.legend()
    # print("save")
    fig.savefig(localization_buffer)#输出为字节流

def generate_rgbframe():
    # global visualize_img
    while True:
        if (visualize_img is None):
            continue
        ret, jpeg = cv2.imencode('.jpeg', visualize_img)
        buffer = jpeg.tobytes()
        yield (b'--frame\r\n'
               b'Content-Type: image/jpeg\r\n\r\n' + buffer + b'\r\n\r\n')

def generate_localization():
    global localization_buffer
    while True:
        plot_data = localization_buffer.getvalue()
        imb = base64.b64encode(plot_data)
        if (len(imb) ==0):
            continue
        # print("localization")
        imd = base64.b64decode(imb)  # base64解码
        yield (b'--frame\r\n'
               b'Content-Type: image/jpeg\r\n\r\n' + imd + b'\r\n\r\n')

def set_command():
    """
    Brief 设置指令
    """
    global WEB_COMMAND,UPDATE_PARAMS
    global left_h_min,left_h_max,right_h_min,right_h_max,s_min,s_max,v_min,v_max
    UPDATE_PARAMS = True
    WEB_COMMAND.left_h_min = int(left_h_min.current_value)
    WEB_COMMAND.left_h_max = int(left_h_max.current_value)
    WEB_COMMAND.right_h_min = int(right_h_min.current_value)
    WEB_COMMAND.right_h_max = int(right_h_max.current_value)
    WEB_COMMAND.s_min = int(s_min.current_value)
    WEB_COMMAND.s_max = int(s_max.current_value)
    WEB_COMMAND.v_min = int(v_min.current_value)
    WEB_COMMAND.v_max = int(v_max.current_value)

threading.Thread(target = lambda :rospy.init_node('web_telemery',anonymous = True,disable_signals=True)).start()
# rospy.Subscriber("/visualize_mat",ROSImage,ImageCallback)
rospy.Subscriber("/visualize_msg",visualize_msg,VisualizeMsgCallback,queue_size=1)
msg_pub = rospy.Publisher('/web_command',web_command_msg,queue_size=1)
# rate = rospy.Rate(10)

#主页，其中kwargs对应html中的变量
@app.route("/")
def index():
    # jinja2模板，具体格式保存在index.html文件中
    return render_template("index.html",
            video_names=["task1"],
            pc_params_info = pc_range_params_info,
            plc_params_info=plc_range_params_info)

@app.route('/setting/<param_name>', methods=["GET"])
def setting(param_name):
    current_value = float(request.args["current_value"])
    range_param = None
    for param_list_name in index_range_param_dict.keys():
        if (param_name in index_range_param_dict[param_list_name]):
            #param_list_name为字典的名称,使用exec应该为：
            #range_param = param_list_name[param_name]
            exec("range_param = "+param_list_name+"[param_name]")
    #range_param = range_param_dict[param_name]
    if range_param is not None:
        range_param.current_value = current_value
        return str(range_param.current_value)
    else:
        return "empty param"


#视频页
@app.route('/video/<name>')
def video(name):
    return render_template("video.html", 
                            name=name,
                            img_params_info = img_range_params_info
                            )

#<!--h3--><!--img src="{{ url_for('video_feed') }}" width="50%"--><!--/h3 -->
#@app.route('/video_feed')  # 这个地址返回视频流响应
#multipart/x-mixed-replace,只支持jpeg...
#视频页推流
@app.route('/video_feed/threshold')
def video_feed():
        return Response(generate_rgbframe(),
                    mimetype='multipart/x-mixed-replace; boundary=frame')

#视频页推流
@app.route('/video_feed/localization')
def localization_feed():
        time.sleep(0.05)
        return Response(generate_localization(),
                    mimetype='multipart/x-mixed-replace; boundary=frame')

#视频页slidebar
@app.route('/video/setting/<param_name>', methods=["GET"])
def img_setting(param_name):
    current_value = float(request.args["current_value"])
    range_param = img_param_dict[param_name]
    if range_param is not None:
        range_param.current_value = current_value
        set_command()
        # print(type(WEB_COMMAND.left_h_min),WEB_COMMAND.left_h_min)
        msg_pub.publish(WEB_COMMAND)
        print("reset command")
        return str(range_param.current_value)
    else:
        print("img param {} is empty".format(param_name))
        return "empty param"

#视频页按键
@app.route('/video/button/<button_name>', methods=["GET"])
def button_press(button_name):
    if (button_name == "stop"):
        WEB_COMMAND.task = 0
        msg_pub.publish(WEB_COMMAND)
        print("stop")
    elif(button_name == "cross"):
        WEB_COMMAND.task = 2
        msg_pub.publish(WEB_COMMAND)
        print("cross")
    else:
        print("unsupported operate")
    
    return ""

#多线程

# def subscribe_thread():
    
#     rospy.Subscriber("/visualize_mat",ROSImage,ImageCallback)
#     rospy.Subscriber("/visualize_msg",visualize_msg,VisualizeMsgCallback)
#     rospy.spin()

# def publish_thread():
#     global msg_pub,rate
#     while (True):
#         if (UPDATE_PARAMS):
#             msg_pub.publish(WEB_COMMAND)
#         rate.sleep() 

def signal_handler(signal,frame):
    rospy.signal_shutdown("end")
    sys.exit(0)
signal.signal(signal.SIGINT,signal_handler)
if __name__ == '__main__':
    
    import sys

    #不注册信号量
    # sub_thread = threading.Thread(target=subscribe_thread)
    # sub_thread.daemon = True
    # sub_thread.start()
    # pub_thread = threading.Thread(target=publish_thread)
    # pub_thread.daemon = True
    # pub_thread.start()
    app.run(host='0.0.0.0', debug=True,port=5000)
    EXIT_SIGNAL = True
    sys.exit()